#pragma config(I2C_Usage, I2C1, i2cSensors)
#pragma config(Sensor, in1,    Potentiometer,  sensorPotentiometer)
#pragma config(Sensor, dgtl1,  front_switch,   sensorTouch)
#pragma config(Sensor, dgtl2,  back_switch,    sensorTouch)
#pragma config(Sensor, I2C_1,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign)
#pragma config(Sensor, I2C_2,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign)
#pragma config(Motor,  port1,           left_motor,    tmotorVex393, openLoop, reversed, encoder, encoderPort, I2C_2, 1000)
#pragma config(Motor,  port6,           claw,          tmotorVex393, openLoop)
#pragma config(Motor,  port7,           arm,           tmotorVex393, openLoop, reversed)
#pragma config(Motor,  port10,          right_motor,   tmotorVex393, openLoop, encoder, encoderPort, I2C_1, 1000)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#define leftEncoder I2C_2
#define rightEncoder I2C_1

#define conversion 0.20101619

#define arm_kp .5

#define motor_kp .5

int oldLeftEncoderCount = 0;
int oldRightEncoderCount = 0;
int leftEncoderCount = 0;
int rightEncoderCount = 0;
float leftError = 0;
float rightError = 0;

void arm_control(int set_point);
void motor_control_speed(int set_point);
void move_distance(float inches, int power);

task main()
{
	SensorValue[I2C_1] = 0;
	SensorValue[I2C_2] = 0;

	while(true)
	{
		arm_control(393);
		motor_control_speed(50);
	}
}




void motor_control_speed(int set_point)
{


	leftEncoderCount = SensorValue[leftEncoder];
	rightEncoderCount = SensorValue[rightEncoder];

	leftError = set_point - (((leftEncoderCount - oldLeftEncoderCount)/time1[T1])*conversion);
	rightError = set_point - (((rightEncoderCount - oldRightEncoderCount)/time1[T1])*conversion);

	motor[left_motor] = motor_kp * leftError;
	motor[right_motor] = motor_kp * rightError;

	oldLeftEncoderCount = leftEncoderCount;
	oldRightEncoderCount = rightEncoderCount;
	ClearTimer(T1);
}

void arm_control(int set_point)
{
	motor[arm]=arm_kp*(set_point-SensorValue[Potentiometer]);
}

void move_distance ( float inches, int power)
{
	float distance=0;
	SensorValue[I2C_1]=0;
	SensorValue[I2C_2]=0;

	if(distance < inches)
	{
		motor[left_motor]=power;
		motor[right_motor]=power;

		distance=(SensorValue[I2C_1]+SensorValue[I2C_2])/2.0*conversion;
	}
	else
	{
		motor[left_motor]=0;
		motor[right_motor]=0;
	}
}

void move_straight ( int power )
{

}
